//
// Created by oftenlin on 23-9-13.
//
#include "ESKFFusionSimple.hpp"
#include "NaviAttitudeTools.hpp"
#include "NaviEarthTools.hpp"
#include <iostream>

void ESKFFusionSimple::Init(NaviData::NavInfo &initPos, uint32_t state_num, uint32_t noise_num) {
    // 安装角偏差
    curNaviInfo_ = initPos;
    preNaviInfo_ = initPos;
    double yaw_bv =  0.63204500000000019 * D2R;
    double pitch_bv = -18.391430300000003 * D2R;
    double roll_bv = 0;
    UtilTools::Attitude::Euler euler{roll_bv,pitch_bv,yaw_bv};
    auto quat_bv = UtilTools::Attitude::Euler2Quaternion(euler);
    curNaviInfo_.cbv_dcm = UtilTools::Attitude::Quaternion2RotationMatrix(quat_bv);
    state_num_ = state_num;
    noise_num_ = noise_num;
    baseQ = Eigen::Matrix<double,12,12>::Zero();

    Eigen::Vector3d acc_vrw {0.001,0.001,0.001};
    Eigen::Vector3d gyr_arw {0.00017,0.00017,0.00017};
    baseQ.block(0, 0, 3, 3) = acc_vrw.cwiseProduct(acc_vrw).asDiagonal();
    // gyro
    baseQ.block(3, 3, 3, 3) = gyr_arw.cwiseProduct(gyr_arw).asDiagonal();
    // bias 的协方差
    Eigen::Vector3d accbias_std {0.001,0.001,0.001};
    Eigen::Vector3d gyrbias_std{1.45444*1e-5,1.45444*1e-5,1.45444*1e-5};
    baseQ.block(6, 6, 3, 3) = gyrbias_std.cwiseProduct(gyrbias_std).asDiagonal() * (2.0 / 3600);
    baseQ.block(9, 9, 3, 3) = accbias_std.cwiseProduct(accbias_std).asDiagonal()* (2.0 / 3600);

    x_ = Eigen::VectorXd(state_num_);
    x_.setZero();

    xp_ = Eigen::MatrixXd(state_num_,state_num_);
    xp_.setZero();
    float P_P_INIT = 32.0f;
    float P_V_INIT = 1.0f;
    float P_A_INIT = 0.0109662;
    float P_AB_INIT = 0.0025;
    float P_GB_INIT = 0;
    // 初始误差
    xp_.block(0,0,3,3) = P_P_INIT * Eigen::Matrix<double,3,3>::Identity();
    xp_.block(3,3,3,3) = P_V_INIT * Eigen::Matrix<double,3,3>::Identity();
    xp_.block(6,6,3,3) = P_A_INIT * Eigen::Matrix<double,3,3>::Identity();
    xp_.block(9,9,3,3) = P_AB_INIT* Eigen::Matrix<double,3,3>::Identity();
    xp_.block(12,12,3,3) = P_GB_INIT * Eigen::Matrix<double,3,3>::Identity();
    // gnss 的精度
    double abx = 0.21057367372617;
    double aby = -0.005898977269151;
    double abz = 0.18103348450862;
    double gbx = -0.0007178014298322;
    double gby = -0.0014217855654507;
    double gbz = 0.0055492960636284;
    acce_bias_ = Eigen::Vector3d {abx, aby, abz};
    gyro_bias_ =  Eigen::Vector3d{gbx,gby,gbz};
    curNaviInfo_.acce_bias_ = acce_bias_;
    curNaviInfo_.gyro_bias_ = gyro_bias_;
    initialized_ = true;
}

void ESKFFusionSimple::Predict(std::shared_ptr<NaviData::ImuData>& imuDataPtr) {
    double dt = imuDataPtr->getImuDataDetail().dt;
    if(dt <= 0){
        return;
    }
    if(!isStartIns){
        isStartIns = true;
        preImu = imuDataPtr->getImuDataDetail();
        return;
    }
    currImu = imuDataPtr->getImuDataDetail();
    // 机械编排
    INSSimple();
    UpdateJacobianMatrix();
    UpdatePredictNoise(dt);
    preImu = currImu;
    preNaviInfo_ = curNaviInfo_;
    double t = imuDataPtr->get_time().getTimeStamps() * 1e-3;
    gtime_t gt{long(t), t - long(t)};
    curNaviInfo_.time_ = gt;
    // IMU 没有更新前不进行速度修正
    isIMUUpdate = true;
}

void ESKFFusionSimple::INSSimple() {
    // 更新加速度计和陀螺仪的偏置
    Eigen::Vector3d delta_angular;
    // 计算角速度增量
    ComputeDeltaAngular(delta_angular);
    // 姿态更新
    Eigen::Quaterniond dq = UtilTools::Attitude::RotationVector2Quaternion(delta_angular);
    curNaviInfo_.quat_ = curNaviInfo_.quat_ * dq;
    curNaviInfo_.att_ = UtilTools::Attitude::Quaternion2Euler(curNaviInfo_.quat_);
    curNaviInfo_.rotation_ = UtilTools::Attitude::Quaternion2RotationMatrix(curNaviInfo_.quat_);
    // 加速度更新
    f_ = currImu.acce_ - curNaviInfo_.acce_bias_;

    Eigen::Vector3d df = curNaviInfo_.rotation_ * f_ ;
    df += grav_;
    // 速度更新
    curNaviInfo_.vel_ += df * currImu.dt;
    // 位置更新
    Eigen::Vector3d dp  = curNaviInfo_.vel_ *  currImu.dt;
    // 将东北天位置转换为经纬度
    auto dri = UtilTools::Earth::DRi(curNaviInfo_.pos_);
    curNaviInfo_.pos_ = curNaviInfo_.pos_ + dri * dp;
}

/**
 * 更精细的机械编排
 */
void ESKFFusionSimple::INSDetail() {
    imuCompensate(preNaviInfo_, currImu);
    velUpdate(preNaviInfo_, curNaviInfo_, preImu, currImu);
    posUpdate(preNaviInfo_, curNaviInfo_, preImu, currImu);
    attUpdate(preNaviInfo_, curNaviInfo_, preImu, currImu);
}

void ESKFFusionSimple::Correct(std::shared_ptr<NaviData::BaseData>& naviDataPtr) {
    if(!isIMUUpdate){
        return;
    }
    if(naviDataPtr->get_type() == NaviData::DataType::GNSSDATA){
        std::shared_ptr<NaviData::GnssData> gnssDataPtr = std::static_pointer_cast<NaviData::GnssData>(naviDataPtr);
        CorrectByGNSSData(gnssDataPtr);
    }
    if(naviDataPtr->get_type() == NaviData::DataType::CANVELOCITYDATA){
        std::shared_ptr<NaviData::CANVelocity> canVelocityDataPtr = std::static_pointer_cast<NaviData::CANVelocity>(naviDataPtr);
        CorrectByCANSpeedData(canVelocityDataPtr);
    }
    FeedBack();
    x_.setZero();
}

void ESKFFusionSimple::GetPos(NaviData::NavInfo &pos) {
    pos = curNaviInfo_;
}
/*雅可比矩阵的更新*/
void ESKFFusionSimple::UpdateJacobianMatrix() {
    // Jacobian
    F_ = Eigen::MatrixXd(state_num_,state_num_);
    F_.setZero();
    // 位置 vs 速度
    F_.block(0,3,3,3) =  Eigen::Matrix<double,3,3>::Identity() ;
    // 速度 vs 姿态
    F_.block(3,6,3,3) =   UtilTools::Attitude::Skew(   preNaviInfo_.rotation_ * (currImu.acce_)) ;
    // 速度 vs 加速度计偏置
    F_.block(3,9,3,3) = 1.0 * preNaviInfo_.rotation_ ;
//    F_.block(6,6,3,3) = -1.0 * UtilTools::Attitude::Skew(w) ;
    // 姿态 vs 陀螺仪偏置
    F_.block(6,12,3,3) =  -1.0 * preNaviInfo_.rotation_;
//    // 误差的一阶 没啥用
//    F_.block(9,9,3,3) = -1.0f/3600*Eigen::Matrix<double,3,3>::Identity();
//    F_.block(12,12,3,3) = -1.0f/3600*Eigen::Matrix<double,3,3>::Identity();
}


bool ESKFFusionSimple::ComputeDeltaAngular(Eigen::Vector3d& delta_angular) {
    double delta_t = currImu.dt;
    if (delta_t <= 0){
        return false;
    }
    Eigen::Vector3d curr_angular_vel = currImu.gyro_ - curNaviInfo_.gyro_bias_;

    Eigen::Vector3d last_angular_vel = preImu.gyro_ - curNaviInfo_.gyro_bias_;
    delta_angular = 0.5 * (curr_angular_vel + last_angular_vel) * delta_t;
    return true;
}

void ESKFFusionSimple::UpdatePredictNoise(double dt) {
    Eigen::Matrix<double,15,15> PHI = Eigen::Matrix<double,15,15>::Identity() ;
    // 离散化 TODO 应该用上一个时刻的 pos
    PHI = PHI +  F_ * dt;
    // Process Noise
    G_ = Eigen::MatrixXd(state_num_,noise_num_);
    G_.setZero();
    G_.block(3,0,3,3) = preNaviInfo_.rotation_ ;
    G_.block(6,3,3,3) = preNaviInfo_.rotation_ ;
    G_.block(9,6,6,6) = Eigen::Matrix<double,6,6>::Identity();
    // Discrete Process Noise
    Q_ = G_*baseQ*G_.transpose() * dt;
    // Covariance Time Update
    xp_ = PHI*xp_*PHI.transpose() + Q_;
    // x_ 更不更新没啥用
    x_ = PHI * x_ ;
}

void ESKFFusionSimple::CorrectByGNSSData(std::shared_ptr<NaviData::GnssData>& gnssDataPtr) {
    std::vector<double> res;
    // Create measurement Y
    Eigen::Vector3d gnss_posion = gnssDataPtr->getGnssObsDetail().pos_;
    // 度转换成米
    auto dr =  UtilTools::Earth::DR(curNaviInfo_.pos_);
    auto cnv = curNaviInfo_.cbv_dcm * (curNaviInfo_.rotation_.transpose());
    Eigen::VectorXd dz = cnv * dr * (curNaviInfo_.pos_ - gnss_posion);
    Eigen::MatrixXd H(3, 15);
    H.setZero();
    H.block(0, 0, 3, 3) = cnv;
    Eigen::MatrixXd R(3, 3);
    R.setZero();

    if (fabs(gnssDataPtr->getGnssObsDetail().mAccuracyLocHorizon) < 1e-6) {
        if (fabs(gnssDataPtr->getGnssObsDetail().mAccuracyLocVertical) < 1e-6) {
            // VDR_LOGI("mAccuracyLocHorizon/Vertical is zero");
            return ;
        } else {
            auto loc_horizon_acc = gnssDataPtr->getGnssObsDetail().mAccuracyLocVertical * 0.6;
            R(0, 0) = pow(loc_horizon_acc, 2) * 0.5;
            R(1, 1) = pow(loc_horizon_acc, 2) * 0.5;
            R(2, 2) = pow(gnssDataPtr->getGnssObsDetail().mAccuracyLocVertical, 2);
        }
    } else {
        R(0, 0) = pow(gnssDataPtr->getGnssObsDetail().mAccuracyLocHorizon, 2) * 0.5;
        R(1, 1) = pow(gnssDataPtr->getGnssObsDetail().mAccuracyLocHorizon, 2) * 0.5;
        if (fabs(gnssDataPtr->getGnssObsDetail().mAccuracyLocVertical) < 1e-6) {
            R(2, 2) = pow(gnssDataPtr->getGnssObsDetail().mAccuracyLocHorizon, 2) * 2.56;
        } else {
            R(2, 2) = pow(gnssDataPtr->getGnssObsDetail().mAccuracyLocVertical, 2);
        }
    }

    // Kalman Gain
    Measurement(dz,H,R,x_,xp_);
}

void ESKFFusionSimple::CorrectByCANSpeedData(std::shared_ptr<NaviData::CANVelocity>& canVelocityPtr ) {
    Eigen::Vector3d odo_vel_obs;
    odo_vel_obs.setZero();
    odo_vel_obs[0] = canVelocityPtr->getCanVelocityDetail().velocity;
    // 得到东北天系 转 车体系的转化矩阵
    Eigen::Matrix3d C_nv = curNaviInfo_.cbv_dcm * (curNaviInfo_.rotation_.transpose());

    auto Cnb = curNaviInfo_.rotation_.transpose();
    // 车体系下的 车速
    Eigen::Vector3d vel_v = C_nv * curNaviInfo_.vel_;

    double odosf = 1.0112411763471918;
    Eigen::VectorXd dz = vel_v - odo_vel_obs * odosf;
    Eigen::MatrixXd H(3, 15);
    H.setZero();
    //  v-v 差一个 n 系统到车体系的旋转矩阵
    H.block(0, 3, 3, 3) = C_nv;
    //  v-att 失准角
    H.block(0, 6, 3, 3) = -1.0 * C_nv * (UtilTools::Attitude::Skew(curNaviInfo_.vel_));
    //  v-bg
    Eigen::MatrixXd R(3, 3);
    R.setZero();
    double odo_vel_std = 0.15;
    for (int i = 0; i < 3; ++i) {
        R(i, i) = pow(odo_vel_std, 2);
    }
    Measurement(dz,H,R,x_,xp_);
}

void ESKFFusionSimple::FeedBack() {
    // 位置补偿 位置由 NED 的米变成度
    auto dri = UtilTools::Earth::DRi(curNaviInfo_.pos_);
    curNaviInfo_.pos_ -= dri * x_.segment(0, 3);
    // 速度补偿
    curNaviInfo_.vel_ -= x_.segment(3, 3);
    // 姿态更新
    Eigen::Quaterniond dq = UtilTools::Attitude::RotationVector2Quaternion(x_.segment(6, 3));
    curNaviInfo_.quat_ =  dq *  curNaviInfo_.quat_ ;
    curNaviInfo_.quat_.normalize();
    curNaviInfo_.att_ = UtilTools::Attitude::Quaternion2Euler(curNaviInfo_.quat_);
    curNaviInfo_.rotation_ = UtilTools::Attitude::Quaternion2RotationMatrix(curNaviInfo_.quat_);
    // 加速度计偏置更新
    curNaviInfo_.acce_bias_ += x_.segment(9,3);
    // 陀螺仪偏置更新
    curNaviInfo_.gyro_bias_ += x_.segment(12,3);
    preNaviInfo_ = curNaviInfo_;
}

void ESKFFusionSimple::posUpdate(const NaviData::NavInfo &pvapre, NaviData::NavInfo &pvacur, const NaviData::ImuData::ImuDataDetail &imupre,
                        const NaviData::ImuData::ImuDataDetail &imucur) {

    Eigen::Vector3d temp1, temp2, midvel, midpos;
    Eigen::Quaterniond qne, qee, qnn;

    // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvacur.vel_ + pvapre.vel_) / 2;
    // pos 是纬 经 高 弧度制 DRi 是把米转换为度
    midpos = pvapre.pos_ + UtilTools::Earth::DRi(pvapre.pos_) * midvel * imucur.dt / 2;

    // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn = UtilTools::Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << constant::constant_wie * cos(midpos[0]), 0, -constant::constant_wie * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
            -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 重新计算 k时刻到k-1时刻 n系旋转矢量
    // recompute n-frame rotation vector (n(k) with respect to n(k-1)-frame)
    temp1 = (wie_n + wen_n) * imucur.dt;
    qnn = UtilTools::Attitude::RotationVector2Quaternion(temp1);
    // e系转动等效旋转矢量 (k-1时刻k时刻，所以取负号)
    // e-frame rotation vector (e(k-1) with respect to e(k)-frame)
    temp2 << 0, 0, -constant::constant_wie * imucur.dt;
    qee =  UtilTools::Attitude::RotationVector2Quaternion(temp2);

    // 位置更新完成
    // position update finish
    qne = UtilTools::Earth::blh2qne(pvapre.pos_);
    qne = qee * qne * qnn;

    pvacur.pos_[2] = pvapre.pos_[2] - midvel[2] * imucur.dt;
    pvacur.pos_ = UtilTools::Earth::qne2blh(qne, pvacur.pos_[2]);
}

void ESKFFusionSimple::velUpdate(const NaviData::NavInfo &pvapre, NaviData::NavInfo &pvacur, const NaviData::ImuData::ImuDataDetail &imupre,
                        const NaviData::ImuData::ImuDataDetail &imucur) {

    Eigen::Vector3d d_vfb, d_vfn, d_vgn, gl, midvel, midpos;
    Eigen::Vector3d temp1, temp2, temp3;
    Eigen::Matrix3d cnn, I33 = Eigen::Matrix3d::Identity();
    Eigen::Quaterniond qne, qee, qnn, qbb, q1, q2;

    // 计算地理参数，子午圈半径和卯酉圈半径，地球自转角速度投影到n系, n系相对于e系转动角速度投影到n系，重力值
    // calculate geographic parameters, Meridian and Mao unitary radii,
    // earth rotational angular velocity projected to n-frame,
    // rotational angular velocity of n-frame to e-frame projected to n-frame, and gravity
    Eigen::Vector2d rmrn =  UtilTools::Earth::meridianPrimeVerticalRadius(pvapre.pos_[0]);
    Eigen::Vector3d wie_n, wen_n;
    wie_n << constant::constant_wie * cos(pvapre.pos_[0]), 0, -constant::constant_wie * sin(pvapre.pos_[0]);
    wen_n << pvapre.vel_[1] / (rmrn[1] + pvapre.pos_[2]), -pvapre.vel_[0] / (rmrn[0] + pvapre.pos_[2]),
            -pvapre.vel_[1] * tan(pvapre.pos_[0]) / (rmrn[1] + pvapre.pos_[2]);
    auto gravity =  UtilTools::Earth::CalculateGravity(pvapre.pos_, false);

    // 旋转效应和双子样划桨效应
    // rotational and sculling motion
    temp1 = (imucur.dtheta).cross(imucur.dvel) / 2;
    temp2 = (imupre.dtheta).cross(imucur.dvel) / 12;
    temp3 = (imupre.dvel).cross(imucur.dtheta) / 12;

    // b系比力积分项
    // velocity increment due to the specific force
    d_vfb = imucur.acce_ * imucur.dt + temp1 + temp2 + temp3;

    // 比力积分项投影到n系
    // velocity increment dut to the specfic force projected to the n-frame
    temp1 = (wie_n + wen_n) * imucur.dt / 2;
    cnn = I33 -  UtilTools::Attitude::Skew(temp1);
    d_vfn = cnn * pvapre.rotation_ * d_vfb;

    // 计算重力/哥式积分项
    // velocity increment due to the gravity and Coriolis force
    gl << 0, 0, gravity[2];
    d_vgn = (gl - (2 * wie_n + wen_n).cross(pvapre.vel_)) * imucur.dt;

    // 得到中间时刻速度
    // velocity at k-1/2
    midvel = pvapre.vel_ + (d_vfn + d_vgn) / 2;

    // 外推得到中间时刻位置
    // position extrapolation to k-1/2
    qnn =  UtilTools::Attitude::RotationVector2Quaternion(temp1);
    temp2 << 0, 0, -constant::constant_wie * imucur.dt / 2;
    qee =  UtilTools::Attitude::RotationVector2Quaternion(temp2);
    qne =  UtilTools::Earth::blh2qne(pvapre.pos_);
    qne = qee * qne * qnn;
    midpos[2] = pvapre.pos_[2] - midvel[2] * imucur.dt / 2;
    midpos =  UtilTools::Earth::qne2blh(qne, midpos[2]);

    // 重新计算中间时刻的rmrn, wie_e, wen_n
    // recompute rmrn, wie_n, and wen_n at k-1/2
    rmrn =  UtilTools::Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << constant::constant_wie * cos(midpos[0]), 0, -constant::constant_wie * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
            -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 重新计算n系下平均比力积分项
    // recompute d_vfn
    temp3 = (wie_n + wen_n) * imucur.dt / 2;
    cnn = I33 -  UtilTools::Attitude::Skew(temp3);
    d_vfn = cnn * pvapre.rotation_ * d_vfb;

    // 重新计算重力、哥式积分项
    // recompute d_vgn
    gl << 0, 0,  UtilTools::Earth::CalculateGravity(midpos, false)[2];
    d_vgn = (gl - (2 * wie_n + wen_n).cross(midvel)) * imucur.dt;

    // 速度更新完成
    // velocity update finish
    pvacur.vel_ = pvapre.vel_ + d_vfn + d_vgn;

}

void ESKFFusionSimple::attUpdate(const NaviData::NavInfo &pvapre, NaviData::NavInfo &pvacur, const NaviData::ImuData::ImuDataDetail &imupre,
                        const NaviData::ImuData::ImuDataDetail &imucur) {

    Eigen::Quaterniond qne_pre, qne_cur, qne_mid, qnn, qbb;
    Eigen::Vector3d temp1, midpos, midvel;

    // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvapre.vel_ + pvacur.vel_) / 2;
    qne_pre =  UtilTools::Earth::blh2qne(pvapre.pos_);
    qne_cur =  UtilTools::Earth::blh2qne(pvacur.pos_);
    temp1 =  UtilTools::Attitude::Quaternion2Vector(qne_cur.inverse() * qne_pre);
    qne_mid = qne_pre *  UtilTools::Attitude::RotationVector2Quaternion(temp1 / 2).inverse();
    midpos[2] = (pvacur.pos_[2] + pvapre.pos_[2]) / 2;
    midpos =  UtilTools::Earth::qne2blh(qne_mid, midpos[2]);

    // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn =  UtilTools::Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << constant::constant_wie * cos(midpos[0]), 0, -(constant::constant_wie) * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
            -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 计算n系的旋转四元数 k-1时刻到k时刻变换
    // n-frame rotation vector (n(k-1) with respect to n(k)-frame)
    temp1 = -(wie_n + wen_n) * imucur.dt;
    qnn =  UtilTools::Attitude::RotationVector2Quaternion(temp1);

    // 计算b系旋转四元数 补偿二阶圆锥误差
    // b-frame rotation vector (b(k) with respect to b(k-1)-frame)
    // compensate the second-order coning correction term.
    temp1 = imucur.dtheta + (imupre.gyro_ * imupre.dt).cross(imucur.dtheta) / 12;
    qbb =  UtilTools::Attitude::RotationVector2Quaternion(temp1);

    // 姿态更新完成
    // attitude update finish
    pvacur.quat_ = qnn * pvapre.quat_ * qbb;
    pvacur.quat_.normalize();
    pvacur.rotation_ =  UtilTools::Attitude::Quaternion2RotationMatrix(pvacur.quat_);
    pvacur.att_ =  UtilTools::Attitude::Quaternion2Euler(pvacur.quat_);
//    pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
}
void ESKFFusionSimple::imuCompensate(NaviData::NavInfo &pvapre, NaviData::ImuData::ImuDataDetail &imucur) {
    // 补偿IMU零偏
    // compensate the imu bias
    imucur.dtheta -= preNaviInfo_.gyro_bias_ * imucur.dt;
    imucur.dvel -= preNaviInfo_.acce_bias_ * imucur.dt;
    for (int i = 0; i < 3; ++i) {
        imucur.gyro_(i) = imucur.dtheta(i) / imucur.dt;
        imucur.acce_(i) = imucur.dvel(i) / imucur.dt;
    }
}

